#include<iostream>
#include<fstream>
#include<iomanip>
#include "Frame.h"
#include "Kdtree_search.h"
#include "chrono"
#include "ransac.h"
#include "triangle.h"
#include "pose_estimate_3d2d.h"
#include "MapPoints.h"
#include "ceresBundle.h"
#include "pcl_viewer.h"

using namespace std;
using namespace Eigen;

void LoadImages(const string &strFile, vector<string> &vstrImageFilenames,
                vector<double> &vTimestamps);
void LoadGroundtruth(const string &poseFile, map<double,Eigen::Isometry3d>& info);

int main(){
    vector<int> test_f; ///<存储测试后选
    vector<string> vstrImageFilenames;
    vector<double> vTimestamps;
    string prefix = "/home/ubuntu/data/rgbd_dataset_freiburg3_long_office_household";
    string strFile = prefix+"/rgb.txt";
    string poseFile = prefix+"/groundtruth.txt";

    Relocation_Slam::ORBVocabulary* mpvocabulary;
    mpvocabulary = new Relocation_Slam::ORBVocabulary();
    bool bVocLoad = mpvocabulary->loadFromBinaryFile("/home/ubuntu/xd/RelocationSlam/ORBvoc.bin");

    cv::Mat img = cv::imread("/home/ubuntu/data/rgbd_dataset_freiburg3_long_office_household/1341847998.586740.png",CV_LOAD_IMAGE_UNCHANGED);
    //1.6642 -0.9328 1.5887 0.7066 0.4673 -0.2894 -0.4456
    Eigen::Isometry3d p_img = Eigen::Isometry3d::Identity();
    Eigen::Vector3d img_t = Eigen::Vector3d(1.6642,-0.9328,1.5887);
    Eigen::Quaterniond img_q(-0.4456,0.7066,0.4673,-0.2894);
    p_img.pretranslate(img_t);
    p_img.rotate(img_q);
    double id = 1341847998.586740;
    Relocation_Slam::Frame img_frame(mpvocabulary,img,p_img,id);

    LoadImages(strFile, vstrImageFilenames, vTimestamps);

    map<double,Eigen::Isometry3d> poseInfo;
    LoadGroundtruth(poseFile, poseInfo);

    int nImages = vstrImageFilenames.size();

    vector<Relocation_Slam::Frame> KeyFrames;
    vector<cv::Point3f> m_pOriPtXYZ;

    cv::Mat im;
    int num=0;
    for(int ni=0; ni<nImages; ni++)
    {
        // Read image from file
        im = cv::imread(prefix+"/"+vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);
        double tframe = floor(vTimestamps[ni] * 100.000f + 0.5) / 100.000f;

        if(poseInfo.count(tframe)){
            num++;
            Relocation_Slam::Frame frame(mpvocabulary,im,poseInfo[tframe],vTimestamps[ni]);
            if(KeyFrames.size()!=0){
                if(pow((poseInfo[tframe].translation()[0]-KeyFrames.back().mPose.translation().x()),2)
                   +pow((poseInfo[tframe].translation()[1]-KeyFrames.back().mPose.translation().y()),2)
                   +pow((poseInfo[tframe].translation()[2]-KeyFrames.back().mPose.translation().z()),2)>0.05*0.05)
                    KeyFrames.push_back(frame);
            }
            else{
                KeyFrames.push_back(frame);
            }

            cv::Point3f p;
            p.x = poseInfo[tframe].translation()[0];
            p.y = poseInfo[tframe].translation()[1];
            p.z = poseInfo[tframe].translation()[2];
            m_pOriPtXYZ.push_back(p);
        }
    }
    std::cout<<"num::"<<num<<endl;
    std::cout<<"KeyFrames::"<<KeyFrames.size()<<endl;

    cv::Mat K = ( cv::Mat_<double> ( 3,3 ) << 535.4, 0, 320.1, 0, 539.2,247.6, 0, 0, 1 );
//    vector<int> indexs;
//    for(int i=0;i<KeyFrames.size();i++){
//        if(pow(KeyFrames[i].mPose.translation().x()-KeyFrames[50].mPose.translation().x(),2)
//            +pow(KeyFrames[i].mPose.translation().y()-KeyFrames[50].mPose.translation().y(),2)
//            +pow(KeyFrames[i].mPose.translation().z()-KeyFrames[50].mPose.translation().z(),2)<5)
//            indexs.push_back(i);
//    }

    cv::Ptr<cv::DescriptorMatcher> matcher  = cv::DescriptorMatcher::create ( "BruteForce-Hamming" );
    vector<cv::DMatch> match;
    vector<cv::DMatch> matches;


    vector<Relocation_Slam::Frame*> relo_frames;
    int frame_id = 1;
//    for(int i=0;i<indexs.size();i++){
    for(int i=0;i<KeyFrames.size();i++){
      //ORB匹配
//        match.clear();
//        matches.clear();
//        matcher->match ( KeyFrames[50].mPointDescriptors,KeyFrames[indexs[i]].mPointDescriptors, match );
//        Mat p10 = imread(prefix+"/rgb/"+to_string(KeyFrames[indexs[i]].mdPicName)+".png",CV_LOAD_IMAGE_UNCHANGED);
//        Mat p20 = imread(prefix+"/rgb/"+to_string(KeyFrames[50].mdPicName)+".png",CV_LOAD_IMAGE_UNCHANGED);
//        Mat img_match;
        //drawMatches (  p20, KeyFrames[50].mvKeyPoints, p10, KeyFrames[indexs[i]].mvKeyPoints,match, img_match );


      //描述子距离信息剔除误匹配
//        DescriptorSelect(KeyFrames[50],match,matches);
//        std::cout<<"match::"<<match.size()<<std::endl;
//        std::cout<<"matches::"<<matches.size()<<std::endl;
//        std::cout<< fixed << setprecision(6)<<KeyFrames[50].mdPicName<<std::endl;
//        std::cout<< fixed << setprecision(6)<<KeyFrames[indexs[i]].mdPicName<<std::endl;


      //ransac方法删除误匹配
//        vector<KeyPoint> RR_KP1;
//        vector<KeyPoint> RR_KP2;
//        std::vector<DMatch> RR_matches;
//        vector<int> f1p_ids;
//        vector<int> f2p_ids;
//        RansacSelect(matches,KeyFrames[50].mvKeyPoints,KeyFrames[indexs[i]].mvKeyPoints,
//                     RR_KP1,RR_KP2,f1p_ids,f2p_ids,RR_matches);
//        Mat img_goodmatch;
//        drawMatches (  p20, RR_KP2,p10, RR_KP1, RR_matches, img_goodmatch );


        //Bow匹配
//        vector<pair<int,int>> match_points;
//        int point_matches =  SelectByBoW(KeyFrames[50],KeyFrames[indexs[i]],match_points,1);

//        if(match_points.size()>20){
            KeyFrames[i].SetFrameId(frame_id);
            Relocation_Slam::Frame* f = &KeyFrames[i];
            relo_frames.push_back(f);
            frame_id++;
//        }
        if(f->mdPicName>1341847998&&f->mdPicName<1341847999){
            test_f.push_back(frame_id-1);
        }
    }

    std::chrono::time_point<std::chrono::high_resolution_clock> p0 = std::chrono::high_resolution_clock::now();

    std::cout<<"relo_frames.size::"<<relo_frames.size()<<std::endl;
    vector<Relocation_Slam::MapPoint> MapPoints;
    //每帧和其前面所有帧进行三角化
    for(int i=1;i<relo_frames.size();i++){
        for(int j=0;j<i;j++){ //j 表示 前面的帧
            vector<pair<int,int>> match_points1; ///< 存储两帧匹配的特征点索引
            int point_matches =  SelectByBoW(*relo_frames[i],*relo_frames[j],match_points1,1);
            vector< cv::Point3f > points_r;

            if(match_points1.size()>15)
                triangulation(relo_frames[i],relo_frames[j],K,match_points1,points_r); ///< 进行三角化

            if(point_matches>15){

                for(int m=0;m<match_points1.size();m++){
                    if(relo_frames[j]->mbKpTriangle[match_points1[m].second] == false
                    &&relo_frames[i]->mbKpTriangle[match_points1[m].first] == false){ ///< 若之前帧和当前帧匹配的特征点没被三角化过
                        cv::Mat p = ( cv::Mat_<float> ( 3,1 ) << points_r[m].x, points_r[m].y, points_r[m].z );
                        Relocation_Slam::MapPoint mp(p,relo_frames[i],points_r.size(),match_points1[m].first); ///< 创建地图点并添加当前帧观测
                        mp.AddObservation(relo_frames[j],match_points1[m].second); ///< 为地图点添加之前帧观测
                        relo_frames[i]->mnMappointId[match_points1[m].first] = MapPoints.size(); ///< 为特征点绑定地图点
                        relo_frames[j]->mnMappointId[match_points1[m].second] = MapPoints.size();
                        relo_frames[i]->mbKpTriangle[match_points1[m].first] = true; ///< 设置已经三角化出地图点
                        relo_frames[j]->mbKpTriangle[match_points1[m].second] = true;
                        MapPoints.push_back(mp);
                    }
                    else if(relo_frames[j]->mbKpTriangle[match_points1[m].second] == true
                            &&relo_frames[i]->mbKpTriangle[match_points1[m].first] == false){ ///< 之前帧的特征点已经三角化过,需要为该地图点添加当前帧对应特征点作为观测
                        int point_id = relo_frames[j]->mnMappointId[match_points1[m].second]; ///< 之前帧的特征点对应的地图点ID
                        MapPoints[point_id].AddObservation(relo_frames[i],match_points1[m].first); ///< 为地图点添加当前帧的观测
                        relo_frames[i]->mnMappointId[match_points1[m].first] = relo_frames[j]->mnMappointId[match_points1[m].second]; ///< 当前帧的特征点绑定地图点
                        relo_frames[i]->mbKpTriangle[match_points1[m].first] = true; ///< 当前帧的特征点已经三角化
                    }
                    else if(relo_frames[j]->mbKpTriangle[match_points1[m].second] == false
                            &&relo_frames[i]->mbKpTriangle[match_points1[m].first] == true){ ///< 当前帧的特征点已经三角化过,需要为该地图点添加之前帧对应特征点作为观测
                        int point_id = relo_frames[i]->mnMappointId[match_points1[m].first]; ///< 当前帧的特征点对应的地图点ID
                        MapPoints[point_id].AddObservation(relo_frames[j],match_points1[m].second); ///< 为地图点添加之前帧的观测
                        relo_frames[j]->mnMappointId[match_points1[m].second] = relo_frames[i]->mnMappointId[match_points1[m].first]; ///< 之前帧的特征点绑定地图点
                        relo_frames[j]->mbKpTriangle[match_points1[m].second] = true; ///< 之前帧的特征点已经三角化
                    }
                }
            }

        }
    }

    std::cout<<"AllMapPoints.SIZE:::"<<MapPoints.size()<<std::endl;

    vector<Relocation_Slam::MapPoint> MapPoints_c = MapPoints;

    SolveProblem(relo_frames,MapPoints);

//    vector<pair<int,int>> match_points2; ///< 存储两帧匹配的特征点索引
//    int point_matches =  SelectByBoW(*relo_frames[3],*relo_frames[11],match_points2,1);
//    cv::Mat R;
//    cv::Mat t;
//    poseEstimate3d2d(
//            *relo_frames[3],
//            *relo_frames[11],
//            match_points2,
//            MapPoints,
//            K,
//            R,
//            t);
//
//    cv::Mat Tinv = (cv::Mat_<double> (4,4) <<
//            R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), t.at<double>(0,0),
//            R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), t.at<double>(1,0),
//            R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), t.at<double>(2,0),
//            0,0,0,1);
//    cv::Mat T = Tinv.inv();
//    cout<<"T:"<<endl<<T<<endl;
//
//    std::chrono::time_point<std::chrono::high_resolution_clock> p1 = std::chrono::high_resolution_clock::now();
//    cout << "time:" << (float)std::chrono::duration_cast<std::chrono::microseconds>(p1 - p0).count() / 1000 << "ms" << endl;
//
//    std::cout<<"relo_frames[11].T.groundTruth:"<<endl<<relo_frames[11]->mPose.matrix()<<endl;

    vector<pair<int,int>> match_points2; ///< 存储两帧匹配的特征点索引
    int point_matches =  SelectByBoW(*relo_frames[test_f[0]],img_frame,match_points2,1);

    cv::Mat R;
    cv::Mat t;
    poseEstimate3d2d(
            *relo_frames[test_f[0]],
            img_frame,
            match_points2,
            MapPoints,
            K,
            R,
            t);


    cv::Mat Tinv = (cv::Mat_<double> (4,4) <<
            R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), t.at<double>(0,0),
            R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), t.at<double>(1,0),
            R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), t.at<double>(2,0),
            0,0,0,1);
    cv::Mat T = Tinv.inv();
    cout<<"T:"<<endl<<T<<endl;

    std::cout<<"img_frame.T.groundTruth:"<<endl<<p_img.matrix()<<endl;

    std::cout<<relo_frames[test_f[0]]->mdPicName<<",,,,,"<<img_frame.mdPicName<<endl;

    char str1[256];
    sprintf(str1, "%lf", relo_frames[test_f[0]]->mdPicName);
    string result1 = str1;

    char str2[256];
    sprintf(str2, "%lf", img_frame.mdPicName);
    string result2= str2;

//    cout<<result1<<",,,"<<result2<<endl;

    string temp = "/home/ubuntu/data/rgbd_dataset_freiburg3_long_office_household/rgb/";
    cv::Mat img_11 = cv::imread ( temp+result1+".png", 1 );
    cv::Mat img_22 = cv::imread ( "/home/ubuntu/data/rgbd_dataset_freiburg3_long_office_household/"+result2+".png", 1 );

    cv::imshow("image_3",img_11);
    cv::imshow("image_11",img_22);
    cv::waitKey();
    pclShow(MapPoints);  ///< 显示所有地图点

    return 0;
}


void LoadImages(const string &strFile, vector<string> &vstrImageFilenames, vector<double> &vTimestamps)
{
    ifstream f;
    f.open(strFile.c_str());

    // skip first three lines
    string s0;
    getline(f,s0);
    getline(f,s0);
    getline(f,s0);
    while(!f.eof())
    {
        string s;
        getline(f,s);
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            double t;
            string sRGB;
            ss >> t;
            vTimestamps.push_back(t);
            ss >> sRGB;
            vstrImageFilenames.push_back(sRGB);
        }
    }
}

void LoadGroundtruth(const string &poseFile, map<double,Eigen::Isometry3d>& info)
{
    ifstream f;
    f.open(poseFile.c_str());

    // skip first three lines
    string s0;
    getline(f,s0);
    getline(f,s0);
    getline(f,s0);
    while(!f.eof())
    {
        string s;
        getline(f,s);
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            double time;
            double p_x,p_y,p_z,r_x,r_y,r_z,r_w;
            ss >> time;
            ss >> p_x;
            ss >> p_y;
            ss >> p_z;
            ss >> r_x;
            ss >> r_y;
            ss >> r_z;
            ss >> r_w;
            time= floor(time * 100.000f + 0.5) / 100.000f;

            Eigen::Vector3d pose_t = Eigen::Vector3d(p_x,p_y,p_z);
            Quaterniond pose_q(r_w,r_x,r_y,r_z);
            Matrix3d pose_r = pose_q.toRotationMatrix();
            Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
            T.rotate(pose_r);
            T.pretranslate(pose_t);
            info.insert(make_pair(time,T));
        }
    }
}